/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <unistd.h>
#include <signal.h>
#include "voslam_image.h"
#include "voslam_blas.h"
#include "voslam_geo.h"
#include "voslam_util.h"
#include "voslam_calc.h"
#include "voslam_bookkeep.h"
#include "voslamdef.h"

#define BASETYPE3 short

static int width;
static int height;
unsigned char *img=NULL,*img1=NULL;
unsigned char *image=NULL,*image1=NULL;
unsigned char *imC=NULL;
int window;

static int numpts=0,*maski,max_num_of_features=300;
static double *XX,*fu,*fv,*iu,*iv,*fu1,*fv1,*iu1,*iv1;

int sockfd,num_landmarks; 
struct timespec start, stop;
static double runtime,param[6],paraml[6],stereo_param[7],X0[10];

int fea_dim=14;
int type;
int radial_distortion=1;
int iter=7;
int num=0;
int end=1;
int step=1;
char *runko="0_kuva";
char *runko1="1_kuva";
int *list;
int mode=0;

int   process_image(unsigned char* image,unsigned char* image1);

/* ./voslam_stereo -m 1000  -n ~/tuli/2007_09_10_stereo_data/panorama/0_kuva -N ~/tuli/2007_09_10_stereo_data/panorama/1_kuva  -i 99 -s 2 -e 243 -w 5  -l /home/janne/tuli/2007_09_10_stereo_data/panorama/index.dat >log111 */
/* ./voslam_stereo -m 1000  -n ~/avant/ajo2/1_kuva  -N ~/avant/ajo2/0_kuva -i 350 -s 2 -e 1080 -p pu1.dat -P pu0.dat -B stereo_paru.dat -w 5  -l /home/janne/avant/ajo2/index.dat >log4 */
/**********************************************************************
 *
 * Color correction functions
 *
 **********************************************************************/

/*
 * Turn a YUV4:2:0 block into an RGB block
 *
 * Video4Linux seems to use the blue, green, red channel
 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 *
 * Color space conversion coefficients taken from the excellent
 * http://www.inforamp.net/~poynton/ColorFAQ.html
 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 * Y values are given for all 4 pixels, but the U (Pb)
 * and V (Pr) are assumed constant over the 2x2 block.
 *
 * To avoid floating point arithmetic, the color conversion
 * coefficients are scaled into 16.16 fixed-point integers.
 * They were determined as follows:
 *
 *	double brightness = 1.0;  (0->black; 1->full scale) 
 *	double saturation = 1.0;  (0->greyscale; 1->full color)
 *	double fixScale = brightness * 256 * 256;
 *	int rvScale = (int)(1.402 * saturation * fixScale);
 *	int guScale = (int)(-0.344136 * saturation * fixScale);
 *	int gvScale = (int)(-0.714136 * saturation * fixScale);
 *	int buScale = (int)(1.772 * saturation * fixScale);
 *	int yScale = (int)(fixScale);	
 */

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

static inline void
move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, 
	       int rowPixels, unsigned char * rgb, int bits)
{
	const int rvScale = 91881;
	const int guScale = -22553;
	const int gvScale = -46801;
	const int buScale = 116129;
	const int yScale  = 65536;
	int r, g, b;

	g = guScale * u + gvScale * v;
//	if (force_rgb) {
//		r = buScale * u;
//		b = rvScale * v;
//	} else {
		r = rvScale * v;
		b = buScale * u;
//	}

	yTL *= yScale; yTR *= yScale;
	yBL *= yScale; yBR *= yScale;

	if (bits == 24) {
		/* Write out top two pixels */
		rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
		rgb[2] = LIMIT(r+yTL);

		rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
		rgb[5] = LIMIT(r+yTR);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 3 * rowPixels;
		rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
		rgb[2] = LIMIT(r+yBL);

		rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
		rgb[5] = LIMIT(r+yBR);
	} else if (bits == 16) {
		/* Write out top two pixels */
		rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) 
			| ((LIMIT(g+yTL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07)
			| (LIMIT(r+yTL) & 0xF8);

		rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) 
			| ((LIMIT(g+yTR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) 
			| (LIMIT(r+yTR) & 0xF8);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 2 * rowPixels;

		rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F)
			| ((LIMIT(g+yBL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07)
			| (LIMIT(r+yBL) & 0xF8);

		rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F)
			| ((LIMIT(g+yBR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07)
			| (LIMIT(r+yBR) & 0xF8);
	}
}

/* Converts from planar YUV420 to RGB24. */
void 
yuv420p_to_rgb(int width, int height,
	       unsigned char *pIn0, unsigned char *pOut0, int bits)
{
	const int numpix = width * height;
	const int bytes = bits >> 3;
	int i, j, y00, y01, y10, y11, u, v;
	unsigned char *pY = pIn0;
	unsigned char *pU = pY + numpix;
	unsigned char *pV = pU + numpix / 4;
	unsigned char *pOut = pOut0;

	for (j = 0; j <= height - 2; j += 2) {
		for (i = 0; i <= width - 2; i += 2) {
			y00 = *pY;
			y01 = *(pY + 1);
			y10 = *(pY + width);
			y11 = *(pY + width + 1);
			u = (*pU++) - 128;
			v = (*pV++) - 128;

			move_420_block(y00, y01, y10, y11, u, v,
				       width, pOut, bits);
	
			pY += 2;
			pOut += 2 * bytes;

		}
		pY += width;
		pOut += width * bytes;
	}
}
/*macro used to convert a YUV pixel to RGB format
  from Bart Nabbe
YUV2BGR*/
#define YUV2RGB(y, u, v, r, g, b)\
  b = y + ((v * 1434) / 2048);\
  g = y - ((u * 406) / 2048) - ((v * 595) / 2048);\
  r = y + ((u * 2078) / 2048);\
  r = r < 0 ? 0 : r;\
  g = g < 0 ? 0 : g;\
  b = b < 0 ? 0 : b;\
  r = r > 255 ? 255 : r;\
  g = g > 255 ? 255 : g;\
  b = b > 255 ? 255 : b


int yuv422torgb (char *YUV, char *RGB, int NumPixels) {
  int i, j;
  register int y0, y1, u, v;
  register int r, g, b;

  for (i = 0, j = 0; i < 2 * NumPixels; i += 4, j += 6)
    {
      u = (unsigned char) YUV[i + 0] - 128;
      y0 = (unsigned char) YUV[i + 1];
      v = (unsigned char) YUV[i + 2] - 128;
      y1 = (unsigned char) YUV[i + 3];
      YUV2RGB (y0, u, v, r, g, b);
      RGB[j + 0] = r;
      RGB[j + 1] = g;
      RGB[j + 2] = b;
      YUV2RGB (y1, u, v, r, g, b);
      RGB[j + 3] = r;
      RGB[j + 4] = g;
      RGB[j + 5] = b;
    }
  return 0;
}

void ppm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
 

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("error\n");
      exit(-1);
    }
  fprintf(fd,"P6\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  fwrite(im,1,3*width*height,fd);

  fclose(fd);
}

void pgm_save(unsigned char* im,char* filename,int width,int height)
{
  FILE* fd;
  int i,j;

  fd=fopen(filename,"wb");
  if(fd==NULL)
    {
      printf("error\n");
      exit(-1);
    } 

  fprintf(fd,"P2\n");
  fprintf(fd,"%d %d\n",width,height);
  fprintf(fd,"255\n");

  for(i=0;i<height;i++)
    {
      for(j=0;j<width;j++)
	{
	  fprintf(fd,"%d ",(int)im[i*(width)+j]);
	}
      fprintf(fd,"\n");
    }
  fclose(fd);
}



void ppm_read(unsigned char *im, char *filename, int *width, int *height)
{
  FILE* fd;
  int i,j,scale,num,reti;
  char *ret;
  char buf[1024];
  
  fd=fopen(filename,"r");
  if(!fd)
  {	
	printf("ppm_read error: could not open file %s\n",filename);
	exit(1);
  }
  ret=fgets(buf,102,fd);
  if(ret<=0)
  {
         printf("ppm_read_i read error\n");
         exit(0);
  }

  if(strncmp(buf,"P2",2)==0)
  {
  	ret=fgets(buf,122,fd);
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	sscanf(buf,"%d",&scale); 
  	for(i=0;i<(*height);i++)
    {
      for(j=0;j<(*width);j++)
	   {
	   		reti=fscanf(fd,"%d",&num);
	   		im[i*(*width)+j]=(unsigned char)num;
	   }
    }
  }
  else if(strncmp(buf,"P5",2)==0)
  {
  	ret=fgets(buf,122,fd);
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	sscanf(buf,"%d %d",width,height);
  	printf("width:%d\nheight:%d\n",*width,*height);
  	ret=fgets(buf,122,fd); 
  	sscanf(buf,"%d",&scale);
  	printf("scale:%d\n",scale); 
  	reti=fread(im,(*height)*(*width),sizeof(char),fd);
  }
   else if(strncmp(buf,"P6",2)==0)
  {
  	ret=fgets(buf,122,fd);
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	sscanf(buf,"%d",&scale);

  	reti=fread(im,3*(*height)*(*width),sizeof(char),fd);
  }
   else if(strncmp(buf,"Y6",2)==0)
  {
  	ret=fgets(buf,122,fd);
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	sscanf(buf,"%d",&scale);

  	reti=fread(im,3*(*height)*(*width)/2,sizeof(char),fd);
  }  
  else if(strncmp(buf,"Y7",2)==0)
  {
  	ret=fgets(buf,122,fd);
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	sscanf(buf,"%d",&scale);

  	reti=fread(im,2*(*height)*(*width),sizeof(char),fd);
  }
  fclose(fd);
//  return im;
}
unsigned char* ppm_read_i(unsigned char *im, char *filename, int *width, int *height, int *type)
{
  FILE* fd;
  int i,j,scale,num,reti;
  char *ret;
  char buf[1024];
  
  fd=fopen(filename,"r");
  if(!fd)
  {	
	printf("ppm_read error: could not open file %s\n",filename);
	exit(1);
  }
  ret=fgets(buf,102,fd);


  if(strncmp(buf,"P2",2)==0)
  {
  	ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d",&scale);
  	if((im=(unsigned char*)malloc((*width)*(*height)*sizeof(char)))==NULL)
    	return(im);

  	for(i=0;i<(*height);i++)
        {
           for(j=0;j<(*width);j++)
	   {
	   		reti=fscanf(fd,"%d",&num);
			if(reti<=0)
                        {
                            printf("ppm_read_i read error\n");
                            exit(0);
                        }
	   		im[i*(*width)+j]=(unsigned char)num;
	   }
    }
    *type=1;
  }
  else if(strncmp(buf,"P5",2)==0)
  {
  	ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
	sscanf(buf,"%d %d",width,height);
  	printf("width:%d\nheight:%d\n",*width,*height);
  	ret=fgets(buf,122,fd); 
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d",&scale);
  	printf("scale:%d\n",scale); 
  	if((im=(unsigned char*)malloc((*width)*(*height)*sizeof(char)))==NULL)
    	return(im);
  	reti=fread(im,(*height)*(*width),sizeof(char),fd);
	if(reti<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
        
	*type=2;
  }
   else if(strncmp(buf,"P6",2)==0)
  {
  	ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
        if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d",&scale);
 	if((im=(unsigned char*)malloc(3*(*width)*(*height)*sizeof(char)))==NULL)
    	return(im);
  	reti=fread(im,3*(*height)*(*width),sizeof(char),fd);
	if(reti<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
     
	*type=3;
  }
   else if(strncmp(buf,"Y6",2)==0)
  {
  	ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
	sscanf(buf,"%d",&scale);
  	if((im=(unsigned char*)malloc(3*(*width)*(*height)*sizeof(char)))==NULL)
    	return(im);

  	reti=fread(im,3*(*height)*(*width)/2,sizeof(char),fd);
	if(reti<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
        
	*type=4;
  }  
  else if(strncmp(buf,"Y7",2)==0)
  {
  	ret=fgets(buf,122,fd);
	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
  	if(buf[0]=='#')
  		ret=fgets(buf,122,fd);
  	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
        sscanf(buf,"%d %d",width,height);
  	ret=fgets(buf,122,fd); 
  	if(ret<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
        sscanf(buf,"%d",&scale);

  	if((im=(unsigned char*)malloc(3*(*width)*(*height)*sizeof(char)))==NULL)
    	return(im);

  	reti=fread(im,2*(*height)*(*width),sizeof(char),fd);
        if(reti<=0)
        {
             printf("ppm_read_i read error\n");
             exit(0);
        }
	*type=5;
  }
  fclose(fd);
  return im;
}

void dist_points4(int *warped, int *pts, double *p, int N)
{
    
    double x,y,r2,r4,r6;
    double u0,v0,f,k1,k2,k3;
    int i;

    f=p[0];
    u0=p[1];
    v0=p[2];
    k1=p[3];
    k2=p[4];
    k3=p[5];
    
    for(i=0;i<N;i++)
    {
        x=(pts[2*i]-u0)/f;
        y=(pts[2*i+1]-v0)/f;
       
        r2=x*x+y*y;
        r4=r2*r2;
        r6=r4*r2;
	
        warped[2*i]=(int)(p[1]+p[0]*(1+p[3]*r2+p[4]*r4+p[5]*r6)*x);
        warped[2*i+1]=(int)(p[2]+p[0]*(1+p[3]*r2+p[4]*r4+p[5]*r6)*y);
    }
}
void captureScene(void) {
    int i;
    char filename[128],filename1[128];


        if(num<end)
        {

            clock_gettime(CLOCK_REALTIME,&start);
            if(mode)
            {
                sprintf(filename,"%s%d.ppm",runko,list[num]);
                sprintf(filename1,"%s%d.ppm",runko1,list[num]);
            }
            else
            {
                sprintf(filename,"%s%d.ppm",runko,num);
                sprintf(filename1,"%s%d.ppm",runko,num);
            }
            printf("%s\n",filename);

            if(type==5)
            {
                if(radial_distortion)
                {
                    ppm_read(image,filename,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        img[i]=image[i*2+1];
                    }
                    ppm_read(image,filename1,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        img1[i]=image[i*2+1];
                    }
                }
                else
                {
                    ppm_read(img,filename,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        image[i]=img[i*2+1];
                    }
                    ppm_read(img,filename1,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        image1[i]=img[i*2+1];
                    }
                }

            }
            else if(type==3)
            {
                if(radial_distortion)
                {
                    ppm_read(image,filename,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        img[i]=image[i*3+1];
                    }
                    ppm_read(image,filename1,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        img1[i]=image[i*3+1];
                    }
                }
                else
                {
                    ppm_read(img,filename,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        image[i]=img[i*3+1];
                    }
                    ppm_read(img,filename1,&width,&height);
                    for(i=0;i<height*width;i++)
                    {
                        image1[i]=img[i*3+1];
                    }
                }

            }
            else
            {
                if(radial_distortion)
                {
                    ppm_read(img,filename,&width,&height);
                    ppm_read(img1,filename1,&width,&height);
                }
                else
                {
                    ppm_read(image,filename,&width,&height);
                    ppm_read(image1,filename1,&width,&height);
                }
            }
            runtime = -(stop.tv_sec - start.tv_sec) - (double)(stop.tv_nsec - start.tv_nsec) / 1000000000.0;

         
           stop=start;
           if(radial_distortion)
           {
              warp_image_fast(image,img,fu,fv,width,height);
              warp_image_fast(image1,img1,fu1,fv1,width,height);
           }
           if(process_image(image,image1)==0)
           {
             num+=step;
             return;
           }
         }

}

    
  
int main(int argc, char* argv[])
{
    double pts[3],ptsw[3];
    int arg,i,j,ret,cnt,maxIt;
    FILE* fd;
    char *x_file="x0.dat";
    char *calib="p.dat";
    char *calib1="pr.dat";
    char *stereo_calib="stereo_par.dat"; 
    char *listfile="list.txt",buf[128],filename[256];


   clock_gettime(CLOCK_REALTIME,&start);
   srand(start.tv_sec);

        list=(int *)malloc(65536*sizeof(int));

    for(arg=1; arg < argc; ++arg)
    {
        char *p=argv[arg];
        if (*p == 0)
            continue;
        if (*p == '-')
        {
            while (*++p)
            {
                switch(*p)
                {
		    case 'h':
		        fprintf(stderr,"\t\n");
                        fprintf(stderr," Usage: ./voslam_stereo [options] \n");
			fprintf(stderr,"[-i num]\t index of the first image to read\n");
			fprintf(stderr,"[-e num]\t index of the last image to read\n");
			fprintf(stderr,"[-s num]\t read every s image\n");
                        fprintf(stderr,"[-m num]\t maximum number of landmarks extracted from single image\n");
			fprintf(stderr,"[-n string]\t path to the images of first camera including the base image name \n");
			fprintf(stderr,"[-N string]\t path to the images of second camera including the base image name \n");
			fprintf(stderr,"[-c  num]\t number of optimisation iterations in slam calculations\n");
			fprintf(stderr,"[-w  num]\t  number of consecutive poses estimated in slam calculations\n");
			fprintf(stderr,"[-p filename]\t filename of file containing 6 calibration parameters for first camera \n");
			fprintf(stderr,"[-P filename]\t filename of file containing 6 calibration parameters for second camera \n");
			fprintf(stderr,"[-B filename]\t filename of file containing 7 parameters for relative pose of stereo cameras\n");
			fprintf(stderr,"[-x filename]\t filename of file containing 7 parameters for initial pose\n");
			fprintf(stderr,"[-l filename]\t filename of file containing indeces of images\n");
			fprintf(stderr,"\t\n");
			return 0;
                    case 'i':
                        num = atoi(*(p+1) ? p+1 : argv[++arg]);
		        end=num+1;
			break;
		    case 'e':
		        end = atoi(*(p+1) ? p+1 : argv[++arg]);
			break;
                    case 'c':
                        iter = atoi(*(p+1) ? p+1 : argv[++arg]);
                        break;
		    case 's':
			step = atoi(*(p+1) ? p+1 : argv[++arg]);
			break;
                    case 'r':
                        radial_distortion=0;
                        break;
  	            case 'w':
			window = atoi(*(p+1) ? p+1 : argv[++arg]);
			break;
                    case 'n':
                        runko = *(p+1) ? p+1 : 
                                argv[++arg];
                        break;
                    case 'N':
                        runko1 = *(p+1) ? p+1 : 
                                argv[++arg];
                        break;
		    case 'p':
			calib = *(p+1) ? p+1 : 
				argv[++arg];
			break;
                    case 'P':
                        calib1 = *(p+1) ? p+1 : 
                                argv[++arg];
                        break;
                    case 'B':
                        stereo_calib = *(p+1) ? p+1 : 
                                argv[++arg];
                        break;
	           case 'm':
			max_num_of_features = atoi(*(p+1) ? p+1 : argv[++arg]);
			break;
   	           case 'x':
			x_file = *(p+1) ? p+1 : 
	                         argv[++arg];
			break;
	           case 'l':
			listfile = *(p+1) ? p+1 : 
				argv[++arg];
                        mode=1;
		        break;
		     }
	          }
	     }
	}

	
	
	if(mode)
	{
		fd=fopen(listfile,"r");
 	 	if(!fd)
  		{	
			printf("error: could not open file %s\n",listfile);
			exit(1);
  		}
                cnt=0;
		while(fgets(buf,102,fd))
		{
			list[cnt]=atoi(buf);
			cnt++;
		}
		fclose(fd);
	}
	if(mode)
	    sprintf(filename,"%s%d.ppm",runko,list[num]);
        else
	    sprintf(filename,"%s%d.ppm",runko,num);
	printf("%s\n",filename);
    img=ppm_read_i(img,filename,&width,&height,&type);
    image=(unsigned char *)malloc(width*height*3*sizeof(unsigned char));
    image1=(unsigned char *)malloc(width*height*3*sizeof(unsigned char));
    img1=(unsigned char *)malloc(width*height*3*sizeof(unsigned char));

        
	if (( maski=(int *)malloc(2*max_num_of_features*sizeof(int))) == NULL) printf("malloc error");
	if (( fu=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( fv=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( iu=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
	if (( iv=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error"); 
    if (( fu1=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
    if (( fv1=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
    if (( iu1=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error");              
    if (( iv1=(double *)malloc(width*height*sizeof(double))) == NULL) printf("malloc error"); 
    maxIt=2*(max_num_of_features+10)*(end-num+step)/step;
    if (( XX=(double *)malloc(maxIt*3*sizeof(double))) == NULL) printf("malloc error");
	fd=fopen(calib,"r");
    if(!fd)
    {   
            printf("error: could not open file %s\n",calib);
            exit(1);
    }
	for(i=0;i<6;i++)
	{
        ret=fscanf(fd,"%lf ",param+i);
        if(ret<=0)
        {
            printf("calib read error\n");
            exit(0);
        }
    }
	fclose(fd);
    fd=fopen(calib1,"r");
    if(!fd)
    {   
            printf("error: could not open file %s\n",calib1);
            exit(1);
    }
            
    for(i=0;i<6;i++)
    {
        ret=fscanf(fd,"%lf ",paraml+i);
        if(ret<=0)
        {
            printf("calib1 read error\n");
            exit(0);
        }
    }
    fclose(fd);
    fd=fopen(stereo_calib,"r");
    if(!fd)
    {   
        printf("error: could not open file %s\n",stereo_calib);
        exit(1);
    }
            
    for(i=0;i<7;i++)
    {
        ret=fscanf(fd,"%lf ",stereo_param+i);
        if(ret<=0)
        {
            printf("stereo_calib read error\n");
            exit(0);
        }
    }
    fclose(fd);

    pts[0]=param[2];
    param[2]=param[1];
    param[1]=pts[0];
    if(radial_distortion)
    {
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
	            pts[1]=j;
	            undist_points3(ptsw,pts,param,10,1);
                iu[i*width+j]=ptsw[0];
	            iv[i*width+j]=ptsw[1];
            }
        }
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
                pts[1]=j;
                dist_points3(ptsw,pts,param,1);
                fu[i*width+j]=ptsw[0];
                fv[i*width+j]=ptsw[1];
            }
        }
    }
    pts[0]=paraml[2];
    paraml[2]=paraml[1];
    paraml[1]=pts[0];
    if(radial_distortion)
    {
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
                pts[1]=j;
                undist_points3(ptsw,pts,paraml,10,1);
                iu1[i*width+j]=ptsw[0];
                iv1[i*width+j]=ptsw[1];
            }
        }
        for(i=0;i<height;i++)
        {
            for(j=0;j<width;j++)
            {
                pts[0]=i;
                pts[1]=j;
                dist_points3(ptsw,pts,paraml,1);
                fu1[i*width+j]=ptsw[0];
                fv1[i*width+j]=ptsw[1];
            }
        }
    }
    fd=fopen(x_file,"r");
    for(i=0;i<7;i++)
    {
        ret=fscanf(fd,"%lf ",X0+i);
        if(ret<=0)
        {
            printf("X0 read error\n");
            exit(0);
        }
    }
    fclose(fd);
    clock_gettime(CLOCK_REALTIME,&stop);	

    
    for(i=0;i<5;i++)
        printf("%lf ",param[i]);
    printf("\n ");
    for(i=0;i<7;i++)
        printf("%lf ",X0[i]);
    printf("\n ");    
    for(;num<end;num+=step)
       captureScene();

    return 0;
}



void init_next_pose(double *v,double *Xpos, double *p, double *res, int N, double th, double *pA, double *XA, double *fA, double *fB,int num_points)
{
    double dev,lambda,dev1,dev0,R[9],v0[10],va[10],norm;
    
    v[0]=Xpos[0];
    v[1]=Xpos[1];
    v[2]=Xpos[2];
    v[3]=Xpos[3];
    norm=sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
    v[0]=v[0]/norm;
    v[1]=v[1]/norm;
    v[2]=v[2]/norm;
    v[3]=v[3]/norm;
    v[4]=Xpos[4];
    v[5]=Xpos[5];
    v[6]=Xpos[6];
    v[7]=p[0];
    v[8]=p[1];
    v[9]=p[2];
    va[7]=p[0];
    va[8]=p[1];
    va[9]=p[2];

    quater_2_rot(R,v);
    p3p2dev(&dev1,res,R,v+4,p,pA,XA,N);

       
    if(N>3)
    {
        ransac_calc_p3p_v(v0,v0+4,v,v+4,0.5,100,p,res,&dev,pA,XA,N,th);

        norm=sqrt(v0[0]*v0[0]+v0[1]*v0[1]+v0[2]*v0[2]+v0[3]*v0[3]);

        quater_2_rot(R,v0);
        p3p2dev(&dev0,res,R,v0+4,p,pA,XA,N);
        if(dev0>2.)
        {
            ransac_calc_p3p_v(v0,v0+4,v,v+4,0.5,100,p,res,&dev,pA,XA,N,th);
            quater_2_rot(R,v0);
            p3p2dev(&dev0,res,R,v0+4,p,pA,XA,N);

        }
        dev=dev0;
        v[0]=v0[0];
        v[1]=v0[1];
        v[2]=v0[2];
        v[3]=v0[3];
        v[4]=v0[4];
        v[5]=v0[5];
        v[6]=v0[6];
        if(dev1<dev)
        {
            v[0]=Xpos[0];
            v[1]=Xpos[1];
            v[2]=Xpos[2];
            v[3]=Xpos[3];
            v[4]=Xpos[4];
            v[5]=Xpos[5];
            v[6]=Xpos[6];
            dev=dev1;
        }

        lambda=10;//0.001
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);

        lambda=0.1;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);

                
        lambda=0.01;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);
        printf("dev: %f \n",dev);
        
        lambda=0.00001;
        pose_calct1(v,pA,XA,&lambda,2.*dev,N);
        quater_2_rot(R,v);
        p3p2dev(&dev,res,R,v+4,p,pA,XA,N);

    }
    if(dev0<dev)
    {
        v[0]=v0[0];
        v[1]=v0[1];
        v[2]=v0[2];
        v[3]=v0[3];
        v[4]=v0[4];
        v[5]=v0[5];
        v[6]=v0[6];
    }

        /* need to check residual after rotation*/
    Xpos[7]=v[0];
    Xpos[7+1]=v[1];
    Xpos[7+2]=v[2];
    Xpos[7+3]=v[3];
    Xpos[7+4]=v[4];
    Xpos[7+5]=v[5];
    Xpos[7+6]=v[6];

            
}
void init_structure(double *v, double *X,double *count,double *Xpos,double *pts1, double *pts2, int *indiX, int  *ind2, int *maski, int N, double thx)
{
    int k;
    
    robust_triangulate(X,Xpos,v,pts1,pts2,ind2,indiX,maski,thx,&k,N);
    count[3]=k;

}


int process_image(unsigned char *simage1,unsigned char *simage2)
{


    static double  *fea,*featuresA,*featuresB,*featuresAl,*featuresBl,*fs;
    static double *X,*resi,*z,*ptsX,vn[9],sume,v[10],vl[10];
    static double th,thx,*Xpos,count[4],*pA,*XA,C[200],W[10],*fA,*fB,*ptA,*ptB;
    static double *xx,*vx,*vv,*icb,*R,*Ce,*L,*x,*g,*nX,*Xout,*e,F[9];
    static double vs[7],vp[7],*depth,lambda;
    static int i=0,j,kk,k,*mask,*indx,*indC,*sindex,*tmp,*swap,cnt,index,*indX,*nind;
    static int wx,wy,wxy,M,N,num_pts,*start,*startl,*ind,*indf,*indz,*indA,*indB,image_ring_size=20;
    static int maxV=700,maxX=50000,maxD=15070,maxI=40000,D,num_frames,frame,maxIt=10;
    static int rwindow,*maskX,*matcho,*matchl,statedim=7,cntr;
    static int Mnext[20],Mprev[20],Mind,stationary;
    static int num_featuresB,num_featuresA,num_featuresBl,num_featuresAl;
    static int *matches,*matchesl,*stereo_matches,scyc,*indc,fhead,num_mpts;
    static BASETYPE1 *w,*qua,*ix;
    static BASETYPE  *images,*imw;
    static BASETYPE2 *patchesA,*patchesB,*patchesAl,*patchesBl;
    static BASETYPE2 *imss,*patches,*res,*reso,*resl,*strengthr,*strengthl,*strength_stereo;

    double numer,den,a1,b1,c1,ee,x1,x2,y1,y2;

    if(qua==NULL)
    {
        
        M=width;
        N=height;
        wx=7;
        wy=7;
        wxy=(2*wx+1)*(2*wy+1);
        Mnext[0]=1;
        Mprev[0]=image_ring_size-1;
        for(j=1;j<image_ring_size-1;j++)
        {
          Mnext[j]=j+1;
          Mprev[j]=j-1;
        }
        Mnext[image_ring_size-1]=0;
        Mprev[image_ring_size-1]=image_ring_size-2;
       
        maxIt=2*(max_num_of_features)*(end-num+step)/step;
        maxV=statedim*window+statedim*2;

        v[7]=param[0];
        v[8]=param[1];
        v[9]=param[2];
        vl[7]=paraml[0];
        vl[8]=paraml[1];
        vl[9]=paraml[2];
        vs[0]=stereo_param[0];
        vs[1]=-stereo_param[2];
        vs[2]=-stereo_param[1];
        vs[3]=-stereo_param[3];
        vs[5]=stereo_param[4];
        vs[4]=stereo_param[5];
        vs[6]=stereo_param[6];
        thx=2;
        th=1.5;

        vn[0]=1.;
        vn[1]=0.;
        vn[2]=0.;
        vn[3]=0.;
        vn[4]=0.;
        vn[5]=0.;
        vn[6]=0.;
        W[0]=1.;
        W[1]=1.;
        W[2]=1.;
        W[3]=1.;
        W[4]=1.;
        W[5]=1.;
        W[6]=1.;
        W[7]=1.;
        W[8]=1.;
        W[9]=1.;
        compose_F(F,vs,vs+4,paraml,param);

        if (( Xpos=(double *)malloc((maxIt*7+maxD)*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( ptsX=(double *)malloc(maxIt*2*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( maskX=(int *)malloc(maxIt*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( xx=(double *)malloc(maxX*3*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( vx=(double *)malloc(maxV*maxX*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( vv=(double *)malloc(maxV*maxV*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( icb=(double *)malloc(maxV*maxX*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( R=(double *)malloc(maxV*maxV*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( Ce=(double *)malloc(maxD*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( x=(double *)malloc(maxD*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( g=(double *)malloc(maxD*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( L=(double *)malloc(3*maxX*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( fea=(double *)malloc(fea_dim*(maxIt)*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( e=(double *)malloc((maxI)*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( z=(double *)malloc(2*(maxI)*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( nind=(int *)malloc(maxI*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( ind=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indf=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indz=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indc=(int *)malloc(2*(maxI)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( depth=(double *)malloc((maxI)*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( patches=(BASETYPE2 *)malloc(wxy*(maxIt)*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( res=(BASETYPE2 *)malloc(max_num_of_features*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( resi=(double *)malloc(max_num_of_features*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( nX=(double *)malloc(max_num_of_features*10*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( indC=(int*)malloc((max_num_of_features)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indx=(int*)malloc((maxIt)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indX=(int*)malloc((maxIt)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( mask=(int*)malloc((max_num_of_features)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( XA=(double *)malloc(maxI*3*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( pA=(double *)malloc(maxI*2*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( X=(double *)malloc(maxD*3*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( matchl=(int *)malloc(maxI*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( resl=(BASETYPE2 *)malloc(maxI*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( matcho=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( reso=(BASETYPE2 *)malloc(2*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( qua=(BASETYPE1  *)malloc(N*M*sizeof(BASETYPE1 ))) == NULL) {printf("malloc error");exit(0);}
        if (( swap=(int *)malloc(N*M*2*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( ix=(BASETYPE1*)malloc(N*M*sizeof(BASETYPE1))) == NULL) {printf("malloc error");exit(0);}
        if (( sindex=(int *)malloc(N*M*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( start=(int *)malloc(( (end-num+step)/step+1)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( startl=(int *)malloc(( (end-num+step)/step+1)*sizeof(int))) == NULL) {printf("malloc error");exit(0);}

        if (( indA=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( indB=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( ptA=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( ptB=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( featuresA=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( featuresB=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( featuresAl=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( featuresBl=(double *)malloc(3*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( fA=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( fB=(double *)malloc(2*max_num_of_features*sizeof(double))) == NULL) {printf("malloc error");exit(0);}
        if (( matches=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( matchesl=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( stereo_matches=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( strengthr=(BASETYPE2 *)malloc(2*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( strengthl=(BASETYPE2 *)malloc(2*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( strength_stereo=(BASETYPE2 *)malloc(2*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( tmp=(int *)malloc(max_num_of_features*sizeof(int))) == NULL) {printf("malloc error");exit(0);}
        if (( patchesA=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( patchesB=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( patchesAl=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}
        if (( patchesBl=(BASETYPE2 *)malloc(wxy*max_num_of_features*sizeof(BASETYPE2))) == NULL) {printf("malloc error");exit(0);}    
        if (( w=(BASETYPE1 *)malloc(N*M*sizeof(BASETYPE1))) == NULL) {printf("malloc error");exit(0);}
        if (( images=(unsigned char *)malloc(width*height*image_ring_size*sizeof(unsigned char)))==NULL) {printf("malloc error");exit(0);}
        if (( imw=(unsigned char *)malloc(width*height*sizeof(unsigned char)))==NULL) {printf("malloc error");exit(0);}
        
        numpts=0;
        detect_corners(simage1,qua,w,M,N); /*can ? get rid of w */

        sp_find_loc_max_4tile(qua,featuresA,10,swap,ix,sindex,&num_featuresA,&k,M,N,max_num_of_features);
        
        sp_extract_patchesn(patchesA,simage1,featuresA,num_featuresA, wx, wy, M);
        
        detect_corners(simage2,qua,w,M,N); /*can ? get rid of w */

        sp_find_loc_max_4tile(qua,featuresAl,10,swap,ix,sindex,&num_featuresAl,&k,M,N,max_num_of_features);
        
        sp_extract_patchesn(patchesAl,simage2,featuresAl,num_featuresAl, wx, wy, M);
        
        frame=0;
        Xpos[0]=X0[0];
        Xpos[1]=X0[1];
        Xpos[2]=X0[2];
        Xpos[3]=X0[3];
        Xpos[4]=X0[4];
        Xpos[5]=X0[5];
        Xpos[6]=X0[6];
        for(i=0;i<max_num_of_features;i++)
            stereo_matches[i]=0;

        match_cornersc(res,strength_stereo,stereo_matches,patchesA,patchesAl,tmp,featuresA,featuresAl,num_featuresA,num_featuresAl,wxy,600);        

        frame=0;
        fhead=0;
        start[0]=fhead;
        for(i=0;i<num_featuresA*wxy;i++)
            patches[i+fhead*wxy]=patchesA[i];
        stereo_init_fea(fea,featuresA,frame+1,0,0,&fhead,num_featuresA);
        startl[0]=fhead;
        for(i=0;i<num_featuresAl*wxy;i++)
            patches[i+fhead*wxy]=patchesAl[i];
        stereo_add_fea(fea,featuresAl,frame+1,1,0,&fhead,num_featuresAl);
        slam_fea_update_stereo_matches(fea,start[0],startl[0],num_featuresA,stereo_matches,strength_stereo);

        slam_stereo_parse_matches(fea,indA,indB,mask,start[0],startl[0],&num_pts);
	for(i=0;i<num_pts;i++)
        {
            ptA[2*i]=fea[indA[i]*fea_dim];
            ptA[2*i+1]=fea[indA[i]*fea_dim+1];
            ptB[2*i]=fea[indB[i]*fea_dim];
            ptB[2*i+1]=fea[indB[i]*fea_dim+1];
            a1=F[0]*ptA[2*i]+F[3]*ptA[2*i+1]+F[6];    
            b1=F[1]*ptA[2*i]+F[4]*ptA[2*i+1]+F[7];
            c1=F[2]*ptA[2*i]+F[5]*ptA[2*i+1]+F[8];
            numer=sqrt((a1*a1+b1*b1));
            den=a1*ptB[2*i]+b1*ptB[2*i+1]+c1;
            ee=den/numer;
            printf("stereoptsA: %d  %d %f %f %f %f %f\n",frame,i,ptA[2*i],ptA[2*i+1],ptB[2*i],ptB[2*i+1],ee);     
        }
        robust_triangulate2(nX,X0,vs,param,paraml,ptA,ptB,indB,indC,mask,thx,&k,num_pts);

        /*
        problem: need to prune A->B, Al->Bl, B->Bl links
        */
        slam_stereo_prune_fea(fea,indB,mask,num_pts);
           
        num_landmarks=1;
        slam_stereo_inc_XX(XX,fea,nX,indC,&num_landmarks,k);

        memcpy(images,simage1,M*N);
        Mind=0;
        
    }
    else
    {

        detect_corners(simage1,qua,w,M,N);
        sp_find_loc_max_4tile(qua,featuresB,1.,swap,ix,sindex,&num_featuresB,&k,M,N,max_num_of_features);
        sp_extract_patchesn(patchesB,simage1,featuresB,num_featuresB, wx, wy, M);
        
        detect_corners(simage2,qua,w,M,N);
        sp_find_loc_max_4tile(qua,featuresBl,1.,swap,ix,sindex,&num_featuresBl,&k,M,N,max_num_of_features);
        sp_extract_patchesn(patchesBl,simage2,featuresBl,num_featuresBl, wx, wy, M);
        
        for(i=0;i<max_num_of_features;i++)
            matches[i]=0;

        match_cornersc(res,strengthr,matches,patchesA,patchesB,tmp,featuresA,featuresB,num_featuresA,num_featuresB,wxy,600);
        
        for(i=0;i<max_num_of_features;i++)
            matchesl[i]=0;

        match_cornersc(res,strengthl,matchesl,patchesAl,patchesBl,tmp,featuresAl,featuresBl,num_featuresAl,num_featuresBl,wxy,600);
        
        for(i=0;i<max_num_of_features;i++)
            stereo_matches[i]=0;

        match_cornersc(res,strength_stereo,stereo_matches,patchesB,patchesBl,tmp,featuresB,featuresBl,num_featuresB,num_featuresBl,wxy,600);

        k=0;
        for(i=0;i<max_num_of_features;i++)
        {
            if(stereo_matches[i]>0)
            {
                x1=featuresB[3*i];
                y1=featuresB[3*i+1];
                x2=featuresBl[3*(stereo_matches[i]-1)];
                y2=featuresBl[3*(stereo_matches[i]-1)+1];
                a1=F[0]*x1+F[3]*y1+F[6];    
                b1=F[1]*x1+F[4]*y1+F[7];
                c1=F[2]*x1+F[5]*y1+F[8];
                numer=sqrt((a1*a1+b1*b1));
                den=a1*x2+b1*y2+c1;
                ee=den/numer;
                if(abs(ee)>0.5)
                {
                     stereo_matches[i]=0;
                     j++;
                }
                else
                     k++;
            }
        }

        j=0;
        stationary=0;
        for(i=0;i<max_num_of_features;i++)
        {
            if(matches[i]>0)
            {
              if(fabs((featuresA[3*i]-featuresB[3*(matches[i]-1)]))+fabs((featuresA[3*i+1]-featuresB[3*(matches[i]-1)+1]))<10)
                stationary++;
              j++;
            }
        }

        if(stationary*10>9*j)
        {
            if((num+2*step)>end)
            {       

                for(i=0;i<frame;i++)
                {
                    printf("Xpos1:");
                    for(j=0;j<statedim;j++)
                    {
                        printf("%f ",Xpos[i*statedim+j]);
                    }
                    printf("\n");
                }
                for(i=0;i<fhead;i++)
                {
                    printf("fea1:");
                    for(j=0;j<fea_dim;j++)
                    {
                        printf("%f ",fea[i*fea_dim+j]);
                    }
                    printf("\n");
                }
            }

            return 0;
        }
        Mind=Mnext[Mind];
        memcpy(images+Mind*M*N,simage1,M*N);
        frame++;


        start[frame]=fhead;
        for(i=0;i<num_featuresB*wxy;i++)
            patches[i+fhead*wxy]=patchesB[i];
        stereo_add_fea(fea,featuresB,frame+1,0,0,&fhead,num_featuresB);
        for(i=0;i<num_featuresBl*wxy;i++)
            patches[i+fhead*wxy]=patchesBl[i];
        startl[frame]=fhead;
        if(fhead>=maxIt)
        {
          printf("error: fhead: %d  is greater than maxIt %d\n",fhead,maxIt);
          exit(1);
        }
        stereo_add_fea(fea,featuresBl,frame+1,1,0,&fhead,num_featuresBl);
        slam_fea_update_stereo_matches(fea,start[frame],startl[frame],num_featuresB,stereo_matches,strength_stereo);
        slam_fea_update_matches(fea,start[frame-1],start[frame],num_featuresA,matches,strengthr);
        slam_fea_update_matches(fea,startl[frame-1],startl[frame],num_featuresAl,matchesl,strengthl);
       
        slam_parse_matches(fea,indA,indB,0,mask,start[frame-1],start[frame],&num_pts);


        for(i=0;i<num_pts;i++)
        {
            ptA[2*i]=fea[indA[i]*fea_dim];
            ptA[2*i+1]=fea[indA[i]*fea_dim+1];
            ptB[2*i]=fea[indB[i]*fea_dim];
            ptB[2*i+1]=fea[indB[i]*fea_dim+1];

        }
        normalise_points(fA,ptA,param,num_pts);
        normalise_points(fB,ptB,param,num_pts);

    
        for(i=0;i<num_landmarks;i++)
            maskX[i]=0;
        k=0;
        cnt=0;
        for(i=0;i<num_pts;i++)
        {
            if(mask[i]==0)
            {
                cnt++;
            }
            else
            {
                index=mask[i];
                index--;
                pA[2*k]=ptB[2*i];
                pA[2*k+1]=ptB[2*i+1];
                XA[3*k]=XX[3*index];
                XA[3*k+1]=XX[3*index+1];
                XA[3*k+2]=XX[3*index+2];
                maskX[index]=1;
                k++;
            }
        }

        
        init_next_pose(v,Xpos+(frame-1)*statedim, param,resi,num_pts-cnt,th,pA,XA,ptA,ptB,num_pts);
        proj3D_err(resi,v,ptB,XX,mask,th,&k,&j,num_pts);
        jsort(resi,j);
        count[2]=k;
        if(k>3)
        { 
            /* refine initial next pose by running slam */
            
            stereo_parse_args_for_slam(fea,XX,z,ind,indf,indc,Xpos+statedim*(frame+1),indx,indX,indz,&j,&kk,fhead,window);
            lambda=0.1;
            if((frame-window)<1)
            {
                rwindow=frame;
                scyc=iter*2;
            }
            else
            {
                rwindow=window;
                scyc=iter;
            }
            D=rwindow*statedim+kk*3;
            printf("D: %d\n",D);
            for(i=0;i<D;i++)
                X[i]=Xpos[i+(frame+1-rwindow)*statedim];
            
            num_frames=rwindow+1;
            
           
            slam3D_stereo_batch(L,R,x,icb,g,param,paraml,vs,&Xout,Xpos,X,ind,indf,indc,z,vv,vx,xx,Ce,C,D,j,num_frames,frame+1-rwindow,lambda,e,.9,scyc);
        
            sume=0;
            for(i=0;i<j;i++)
                sume=sume+e[i];
         
            hpsrt(e,indx,j);
           for(i=0;i<D;i++)
                Xpos[i+(frame+1-rwindow)*statedim]=Xout[i];
        
            update_XX(XX,Xpos+statedim*(frame+1),indX,kk);
        
            slam3D_stereo_er(param,paraml,vs,Xpos,Xout,ind,indf,indc,z,D,j,num_frames,frame+1-rwindow,&sume,e,depth);
        
            cntr=0;
            for(i=0;i<j;i++)
            {
                if(e[i]>3*th)
                    cntr++;
            }
         
            slam_stereo_prune_slamfea(fea,indz,e,1.*th,j);
            /* find rematches ..
             * because it searches matches in reverse order it doesn't need to care if track with nonzero next is
             * not looked at because its projection already was checked previously 
             * needs work, same for second camera etc.
            */
                    j=0;
            cntr=0;
            for(i=start[frame];i>start[frame-rwindow];i--)
            {
                index=fea[i*fea_dim+6];
                if((index>-1)&&(fea[i*fea_dim+2]==-1)&&(fea[i*fea_dim+4]==0))
                {
                    index=index-1;
                    //need to be nearestpoints
                    slam_find_nearest_point_to_landmark(v,param,fea+start[frame]*fea_dim, XX+index*3,resi,ind,num_featuresB, M,N);
                    if(resi[0]<4)
                    {
                        res[0]=match_features2(patches+i*wxy,patches+(start[frame]+ind[0])*wxy,wxy);
                        printf("rem: %d %d %f %f\n",num,i,res[0],resi[0]);
                        if(res[0]>0.90)
                        {
                            fea[2+i*fea_dim]=start[frame]+ind[0];      // next
                            fea[3+(start[frame]+ind[0])*fea_dim]=i;      // prev 
                            fea[6+(start[frame]+ind[0])*fea_dim]=fea[6+i*fea_dim];  // map 
                            j++;
                        }
                    }
                    if(resi[0]<900)
                        cntr++;    
                }
            }
            
            /* initialise new landmarks based on left camera */
            slam_parse_matches(fea,indA,indB,0,mask,start[frame-1],start[frame],&num_pts);
            numpts=num_pts;

         
            j=0;
            for(i=0;i<num_pts;i++)
            {
                ptA[2*i]=fea[indA[i]*fea_dim];
                ptA[2*i+1]=fea[indA[i]*fea_dim+1];
                ptB[2*i]=fea[indB[i]*fea_dim];
                ptB[2*i+1]=fea[indB[i]*fea_dim+1];
                if(mask[i]==0)
                    j++;
  
            }
            proj3D_err(resi,v,ptB,XX,mask,th,&k,&j,num_pts);
            jsort(resi,j);

        
            init_structure(v,nX,count,Xpos+(frame-1)*statedim,ptA, ptB,indC,indB,mask,num_pts,thx);
            
            Xout=Xpos+(frame-1)*statedim;
            k=count[3];  
            for(i=0;i<num_pts;i++)
                maski[i]=mask[i];
            slam_prune_fea(fea,indB,mask,num_pts);
            slam_inc_XX(XX,fea,nX,indC,&num_landmarks,k);
            if(num_landmarks>=maxIt)
            {
                printf("error: num_landmarks: %d  is greater than maxIt %d\n",num_landmarks,maxIt);
                exit(1);
            }
            /* initialise new landmarks based on right? camera */
            slam_parse_matches(fea,indA,indB,1,mask,startl[frame-1],startl[frame],&num_mpts);

  
            for(i=0;i<num_mpts;i++)
            {
                ptA[2*i]=fea[indA[i]*fea_dim];
                ptA[2*i+1]=fea[indA[i]*fea_dim+1];
                ptB[2*i]=fea[indB[i]*fea_dim];
                ptB[2*i+1]=fea[indB[i]*fea_dim+1];

            }
	    
            motion3D_update(Xpos+(frame-1)*statedim,vs,vp);
            motion3D_update(v,vs,vn);
            robust_triangulate2(nX,vp,vn,paraml,paraml,ptA,ptB,indB,indC,mask,thx,&k,num_mpts);
            
	    slam_prune_fea(fea,indB,mask,num_mpts);
            slam_inc_XX(XX,fea,nX,indC,&num_landmarks,k);

            /* initialise new landmarks based on stereo camera */
            slam_stereo_parse_matches(fea,indA,indB,mask,start[frame],startl[frame],&num_mpts);

            robust_triangulate2(nX,v,vn,param,paraml,ptA,ptB,indB,indC,mask,thx,&k,num_mpts);

        /*
        problem: need to prune A->B, Al->Bl, B->Bl links
        */
            slam_stereo_prune_fea(fea,indB,mask,num_mpts);
            slam_stereo_inc_XX(XX,fea,nX,indC,&num_landmarks,k);
            /* run slam with new landmarks */
            stereo_parse_args_for_slam(fea,XX,z,ind,indf,indc,Xpos+statedim*(frame+1),indx,indX,indz,&j,&kk,fhead,window);
            
            lambda=0.1;
            if((frame-window)<1)
            {
                rwindow=frame;
                scyc=iter*2;
            }
            else
            {
                rwindow=window;
                scyc=iter;
            }

            D=rwindow*statedim+kk*3;
            printf("D: %d\n",D);
            
            for(i=0;i<D;i++)
                X[i]=Xpos[i+(frame+1-rwindow)*statedim];
            num_frames=rwindow+1;
      
            if(kk>=maxD)
            {
                printf("error: kk: %d  is greater than maxIt %d\n",kk,maxD);
                exit(1);
            }
            if(j>=maxI)
            {
                printf("error: j: %d  is greater than maxI %d\n",j,maxI);
                exit(1);
            }

            slam3D_stereo_batch(L,R,x,icb,g,param,paraml,vs,&Xout,Xpos,X,ind,indf,indc,z,vv,vx,xx,Ce,C,D,j,num_frames,frame+1-rwindow,lambda,e,.9,scyc);
               sume=0;
            for(i=0;i<j;i++)
                sume=sume+e[i];

        
            hpsrt(e,indx,j);
            /* unpack newly estimated poses and landmarks coordinates from estimattion vector */
            for(i=0;i<D;i++)
                Xpos[i+(frame+1-rwindow)*statedim]=Xout[i];
            update_XX(XX,Xpos+statedim*(frame+1),indX,kk);

        }
        else
        {
           
           /* we have failed to find old landmarks in the image due to either they aren't there of the slam algoririthm has failed
           now we reinitialise based on stereo data and settting the initial pose to last good pose 
            * /  
         * }
        motion3D_update(Xpos+(frame-1)*statedim,vs,vp);
        motion3D_update(v,vs,vn);
        */  

            k=0;
            /*erase stray tracks */
            for(i=fhead;i>start[frame];i--)
            {
                fea[fea_dim*i+3]=-1;
                fea[fea_dim*i+6]=-1;
                k++;
            }
            for(i=0;i<statedim;i++)
            {
                Xpos[frame*statedim+i]=Xpos[(frame-1)*statedim+i];
                v[i]=Xpos[(frame-1)*statedim+i];
            }
            /* initialise new landmarks based on stereo camera */
            slam_stereo_parse_matches(fea,indA,indB,mask,start[frame],startl[frame],&num_mpts);
             j=0;
            k=0;
            for(i=0;i<num_mpts;i++)
            {
                ptA[2*i]=fea[indA[i]*fea_dim];
                ptA[2*i+1]=fea[indA[i]*fea_dim+1];
                ptB[2*i]=fea[indB[i]*fea_dim];
                ptB[2*i+1]=fea[indB[i]*fea_dim+1];
                a1=F[0]*ptA[2*i]+F[3]*ptA[2*i+1]+F[6];    
                b1=F[1]*ptA[2*i]+F[4]*ptA[2*i+1]+F[7];
                c1=F[2]*ptA[2*i]+F[5]*ptA[2*i+1]+F[8];
                numer=sqrt((a1*a1+b1*b1));
                den=a1*ptB[2*i]+b1*ptB[2*i+1]+c1;
                ee=den/numer;
                if(abs(ee)<0.5)
                {

                     j++;
                }
                printf("stereoptsA: %d  %d %f %f %f %f %f \n",frame,i,ptA[2*i],ptA[2*i+1],ptB[2*i],ptB[2*i+1],ee);
                if(mask[i]==0)
                    k++;
            }
             motion3D_update(v,vs,vn);
            robust_triangulate2(nX,v,vn,param,paraml,ptA,ptB,indB,indC,mask,thx,&k,num_mpts);

        /*
        problem: need to prune A->B, Al->Bl, B->Bl links
        */
            slam_stereo_prune_fea(fea,indB,mask,num_mpts);
            slam_stereo_inc_XX(XX,fea,nX,indC,&num_landmarks,k);

            numpts=0;

        }
        fs=featuresA;
        featuresA=featuresB;
        featuresB=fs;
        num_featuresA=num_featuresB;
        imss=patchesA;
        patchesA=patchesB;
        patchesB=imss;
        fs=featuresAl;
        featuresAl=featuresBl;
        featuresBl=fs;
        num_featuresAl=num_featuresBl;
        imss=patchesAl;
        patchesAl=patchesBl;
        patchesBl=imss;

        if((num+2*step)>end)
        {       

            for(i=0;i<=frame;i++)
            {
                printf("Xpos1:");
                for(j=0;j<statedim;j++)
                {
                    printf("%f ",Xpos[i*statedim+j]);
                }
                printf("\n");
            }
            for(i=0;i<fhead;i++)
            {
                printf("fea:");
                for(j=0;j<fea_dim;j++)
                {
                    printf("%f ",fea[i*fea_dim+j]);
                }
                printf("\n");
            }
        }
    }
    return 1;
}
